#include "PassiveWalker.h"
#include "PassiveWalkerDef.h"

PassiveWalker::PassiveWalker(b2World* w, const b2Vec2& position)				
{
	m_world = w;
	
	PassiveWalkerDef def;
	b2BodyDef bd;
	b2BodyDef bdLFoot;
	b2BodyDef bdLCalf;
	b2BodyDef bdLThigh;

	// create body parts

	bd = def.PelvisDef;
	bd.angle = 0.0f;
	bd.position += position;
	Pelvis = w->CreateBody(&bd);
	Pelvis->CreateShape(&def.PelvisPoly);
	Pelvis->SetMassFromShapes(1.5f);
	
	bd = def.RThighDef;
	bd.angle = 0.0f;//-0.5f*b2_pi;
	bd.position += position;
	RThigh = w->CreateBody(&bd);
	RThigh->CreateShape(&def.RThighPoly);
	RThigh->SetMassFromShapes(1.0f);

	bd = def.RCalfDef;
	bd.angle = RThigh->GetAngle();
	bd.position += position;
	RCalf = w->CreateBody(&bd);
	RCalf->CreateShape(&def.RCalfPoly);
	RCalf->SetMassFromShapes(0.4f);

	bd = def.RFootDef;
	bd.angle = RCalf->GetAngle();
	bd.position += position;
	RFoot = w->CreateBody(&bd);
	RFoot->CreateShape(&def.RFootPoly);
	RFoot->SetMassFromShapes(0.4f);

	bdLThigh = def.LThighDef;
	bdLThigh.angle = 0.0f;//0.5f*b2_pi;	
	bdLThigh.position += position;
	LThigh = w->CreateBody(&bdLThigh);
	LThigh->CreateShape(&def.LThighPoly);
	LThigh->SetMassFromShapes(1.0f);
	LThigh->SetXForm(position, 0.5f*b2_pi);

	bdLCalf = def.LCalfDef;
	bdLCalf.angle = LThigh->GetAngle();
	bdLCalf.position += position;
	LCalf = w->CreateBody(&bdLCalf);
	LCalf->CreateShape(&def.LCalfPoly);
	LCalf->SetMassFromShapes(0.4f);

	bdLFoot = def.LFootDef;
	bdLFoot.angle = LCalf->GetAngle();
	bdLFoot.position += position;
	LFoot = w->CreateBody(&bdLFoot);
	LFoot->CreateShape(&def.LFootPoly);
	LFoot->SetMassFromShapes(0.4f);


	
	/*def.LAnkleDef.Initialize(LFoot, LCalf, b2Vec2(bdLFoot.position.x+cosf(bdLFoot.angle),bdLFoot.position.y+sinf(bdLFoot.angle)));
	def.LKneeDef.Initialize(LCalf, LThigh, b2Vec2(bdLCalf.position.x+cosf(bdLCalf.angle),bdLCalf.position.y+sinf(bdLCalf.angle)));
	def.LHipDef.Initialize(LThigh, Pelvis, b2Vec2(bdLThigh.position.x+cosf(bdLThigh.angle),bdLThigh.position.y+sinf(bdLThigh.angle)));
	*/
	def.RHipDef.body1		= RThigh;
	def.RHipDef.body2		= Pelvis;
	def.RKneeDef.body1		= RCalf;
	def.RKneeDef.body2		= RThigh;
	def.RAnkleDef.body1		= RFoot;
	def.RAnkleDef.body2		= RCalf;
	

	def.LHipDef.body1		= LThigh;
	def.LHipDef.body2		= Pelvis;
	def.LKneeDef.body1		= LCalf;
	def.LKneeDef.body2		= LThigh;
	def.LAnkleDef.body1		= LFoot;
	def.LAnkleDef.body2		= LCalf;

	// create joints
	

	RAnkle		= (b2RevoluteJoint*)w->CreateJoint(&def.RAnkleDef);
	RKnee		= (b2RevoluteJoint*)w->CreateJoint(&def.RKneeDef);
	RHip		= (b2RevoluteJoint*)w->CreateJoint(&def.RHipDef);
	LAnkle		= (b2RevoluteJoint*)w->CreateJoint(&def.LAnkleDef);
	LKnee		= (b2RevoluteJoint*)w->CreateJoint(&def.LKneeDef);
	LHip		= (b2RevoluteJoint*)w->CreateJoint(&def.LHipDef);

		

}


PassiveWalker::~PassiveWalker(void)
{
	m_world->DestroyBody(LFoot);
	m_world->DestroyBody(RFoot);
	m_world->DestroyBody(LCalf);
	m_world->DestroyBody(RCalf);
	m_world->DestroyBody(LThigh);
	m_world->DestroyBody(RThigh);
	m_world->DestroyBody(Pelvis);
/*	m_world->DestroyBody(Stomach);
	m_world->DestroyBody(Chest);
	m_world->DestroyBody(Neck);
	m_world->DestroyBody(Head);
	m_world->DestroyBody(LUpperArm);
	m_world->DestroyBody(RUpperArm);
	m_world->DestroyBody(LForearm);
	m_world->DestroyBody(RForearm);
	m_world->DestroyBody(LHand);
	m_world->DestroyBody(RHand);
	*/
}
